
#include <queue>
#include <limits>
#include <algorithm>
#include <iomanip>

#include "dijkstra.h"
#include "map.h"

/**
 * 判断三个点组成的路径方向是否发生改变
 *
 * @param prev 上一个点的位置
 * @param curr 当前点的位置
 * @param next 下一个点的位置
 * @return 如果方向发生改变，返回true；否则返回false
 *
 * 方向改变的定义是基于两个连续向量的非平行性：
 * - vec1 为当前点与上一个点构成的向量
 * - vec2 为下一个点与当前点构成的向量
 * 如果vec1在x轴上有变化而vec2在y轴上有变化，或者vec1在y轴上有变化而vec2在x轴上有变化，
 * 则认为路径的方向发生了改变。这是因为这样的条件表明两个向量不是平行的，即路径发生了转折。
 */
bool isDirectionChanged(const Point& prev, const Point& curr, const Point& next) {
    Point vec1 = curr - prev;
    Point vec2 = next - curr;
    return (vec1.x != 0 && vec2.y != 0) || (vec1.y != 0 && vec2.x != 0);
}

// 获取指定位置的邻居节点
// 参数pos: 表示当前位置的Point对象
// 参数grid: 表示网格的二维向量，其中包含不同类型的栅格
// 返回值: 包含当前位置邻居节点的向量
std::vector<Point> getNeighbors(const Point& pos, const std::vector<std::vector<int>>& grid) {
    std::vector<Point> neighbors;
    // 获取网格的行数和列数
    int rows = grid.size();
    int cols = grid[0].size();

    int x = pos.x;
    int y = pos.y;

    // 边界检测
    if (x < 0 || x >= cols || y < 0 || y >= rows) {
        std::cerr << "avilid point" << x << " " << y << std::endl;
        return {};
    }

    // 如果当前位置是交叉点，则使用四领域节点
    if (grid[y][x] == GridType::kCross) {
        // 检查上、下、左、右四个方向的节点，如果不是障碍物，则添加到邻居列表中
        if (x > 0 && grid[y][x - 1] != GridType::kObstacle)
            neighbors.emplace_back(x - 1, y);
        if (x < cols - 1 && grid[y][x + 1] != GridType::kObstacle)
            neighbors.emplace_back(x + 1, y);
        if (y > 0 && grid[y - 1][x] != GridType::kObstacle)
            neighbors.emplace_back(x, y - 1);
        if (y < rows - 1 && grid[y + 1][x] != GridType::kObstacle)
            neighbors.emplace_back(x, y + 1);
    }  // 如果当前位置是普通节点，则使用二领域节点
       // 检查左右两个方向的节点，如果不是障碍物，则添加到邻居列表中
    else if (grid[y][x] == GridType::kNormal) {
        if (y > 0 && grid[y - 1][x] != GridType::kObstacle)
            neighbors.emplace_back(x, y - 1);
        if (y < rows - 1 && grid[y + 1][x] != GridType::kObstacle)
            neighbors.emplace_back(x, y + 1);
    }

    return neighbors;
}

/**
 * Dijkstra路径规划算法实现
 *
 * @param grid 二维网格表示的地图，0表示可通过，非0表示障碍物
 * @param start 起始点坐标
 * @param goal 目标点坐标
 * @return 返回从起始点到目标点的路径，如果无法到达则返回空路径
 */
std::vector<Point> DijkstraPlan(const std::vector<std::vector<int>>& grid, const Point& start, const Point& goal) {
    // 获取地图的行数和列数
    int rows = grid.size();
    int cols = grid[0].size();

    // 优先队列，用于存储待探索的节点，按照节点的代价从小到大排序
    std::priority_queue<Node, std::vector<Node>, std::greater<Node>> openSet;
    // 存储已探索的节点，避免重复探索
    std::vector<std::vector<bool>> closedSet(rows, std::vector<bool>(cols, false));
    // 存储从起始点到每个点的最小代价
    std::vector<std::vector<int>> minCost(rows, std::vector<int>(cols, 300));
    // 存储从起始点到每个点的最小转弯次数
    std::vector<std::vector<int>> minTurns(rows, std::vector<int>(cols, 300));

    // 将起始点加入开放集，初始代价和转弯次数均为0
    openSet.emplace(start, 0, 0, nullptr);
    minCost[start.y][start.x]  = 0;
    minTurns[start.y][start.x] = 0;

    // 主循环，直到开放集为空或找到路径
    while (!openSet.empty()) {
        Node current = openSet.top();
        openSet.pop();
        // 如果当前节点已经探索过，则跳过
        if (closedSet[current.position.y][current.position.x])
            continue;

        // 将当前节点标记为已探索
        closedSet[current.position.y][current.position.x] = true;
        // 如果到达目标点，构建路径并返回
        if (current.position == goal) {
            std::vector<Point> path;
            // 从当前节点回溯到起始点，构建路径
            while (current.parent != nullptr) {
                path.emplace_back(current.position);
                current = *current.parent;
            }
            path.emplace_back(start);
            // 反转路径，使其从起始点到目标点
            std::reverse(path.begin(), path.end());
            return path;
        }
        // 获取当前节点的邻居节点
        std::vector<Point> neighbors = getNeighbors(current.position, grid);
        for (const auto& neighbor : neighbors) {
            // 如果邻居节点已经探索过，则跳过
            if (closedSet[neighbor.y][neighbor.x]) {
                continue;
            }

            // 计算到达邻居节点的新代价和新转弯次数
            int newCost      = current.cost + 1;
            int newTurnCount = current.turnCount;

            // 如果当前节点有父节点，判断是否需要增加转弯次数
            if (current.parent != nullptr) {
                if (isDirectionChanged(current.parent->position, current.position, neighbor)) {
                    newTurnCount++;
                }
            }
            // 如果新代价更小，或者代价相同但转弯次数更少，则更新邻居节点的代价和转弯次数，并将其加入开放集
            if (newCost < minCost[neighbor.y][neighbor.x] ||
                (newCost == minCost[neighbor.y][neighbor.x] && newTurnCount < minTurns[neighbor.y][neighbor.x])) {
                minCost[neighbor.y][neighbor.x]  = newCost;
                minTurns[neighbor.y][neighbor.x] = newTurnCount;

                // 创建新节点并加入开放集
                openSet.emplace(neighbor, newCost, newTurnCount,
                                new Node(current.position, current.cost, current.turnCount, current.parent));
            }
        }
    }

    return {};  // 未找到路径
}
